#include "stdio.h"
#include "pinctrl.h"
#include "gpio.h"
#include "i2c.h"
#include "securec.h"
#include "MPU6050.h"
#include "MPU6050_Reg.h"



#define AP3216C_RESET_TIME 50


// 向从机设备 发送数据
uint32_t MPU6050_WiteByteData(uint8_t byte)
{
    uint8_t buffer[] = {byte};
    i2c_data_t i2cData = {0};
    i2cData.send_buf = &byte;
    i2cData.send_len = sizeof(buffer);
    return uapi_i2c_master_write(MPU6050_I2C_IDX, MPU6050_ADDRESS, &i2cData);
}
// 读从机设备数据
uint32_t MPU6050_RecvData(uint8_t *data, size_t size)
{
    i2c_data_t i2cData = {0};
    i2cData.receive_buf = data;
    i2cData.receive_len = size;

    return uapi_i2c_master_read(MPU6050_I2C_IDX, MPU6050_ADDRESS, &i2cData);
}

// 向寄存器中写数据
 uint32_t MPU6050_WiteCmdByteData(uint8_t regAddr, uint8_t byte)
{
    uint8_t buffer[] = {regAddr, byte};
    i2c_data_t i2cData = {0};
    i2cData.send_buf = buffer;
    i2cData.send_len = sizeof(buffer);
    return uapi_i2c_master_write(MPU6050_I2C_IDX, MPU6050_ADDRESS, &i2cData);
}

// 读寄存器中的数据   参数: regAddr 目标寄存器地址, byte: 取到的数据
uint32_t MPU6050_ReadRegByteData(uint8_t regAddr, uint8_t *byte)
{
    uint32_t result = 0;
    uint8_t buffer[2] = {0};

    // 写命令
    result = MPU6050_WiteByteData(regAddr);
    if (result != ERRCODE_SUCC) {
        printf("I2C AP3216C status = 0x%x!!!\r\n", result);
        return result;
    }

    // 读数据
    result = MPU6050_RecvData(buffer, 1);
    if (result != ERRCODE_SUCC) {
        printf("I2C AP3216C status = 0x%x!!!\r\n", result);
        return result;
    }
    *byte = buffer[0];

    return ERRCODE_SUCC;
}




uint32_t MPU6050_Init(void)
{
    uint32_t result;
    uint32_t baudrate = MPU6050_I2C_SPEED;
    uint32_t hscode = I2C_MASTER_ADDR;
    uapi_pin_set_mode(I2C_SCL_MASTER_PIN, CONFIG_PIN_MODE);
    uapi_pin_set_mode(I2C_SDA_MASTER_PIN, CONFIG_PIN_MODE);
    uapi_pin_set_pull(I2C_SCL_MASTER_PIN, PIN_PULL_TYPE_UP);
    uapi_pin_set_pull(I2C_SDA_MASTER_PIN, PIN_PULL_TYPE_UP);
    
    result = uapi_i2c_master_init(MPU6050_I2C_IDX, baudrate, hscode);
    
    if (result != ERRCODE_SUCC) {
        printf("I2C Init status is 0x%x!!!\r\n", result);
        return result;
    }
    
    MPU6050_WiteCmdByteData(0x6B,0x01);
    osDelay(100);  // 等待稳定（需>5ms）
    MPU6050_WiteCmdByteData(MPU6050_PWR_MGMT_2,0x00);
    MPU6050_WiteCmdByteData(MPU6050_SMPLRT_DIV,0x09);
    MPU6050_WiteCmdByteData(MPU6050_CONFIG,0x06);
    MPU6050_WiteCmdByteData(MPU6050_GYRO_CONFIG,0x18);
    MPU6050_WiteCmdByteData(MPU6050_ACCEL_CONFIG,0x18);
    

    return ERRCODE_SUCC;
}


/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号
  */
void MPU6050_GetID(uint8_t *byte)
{
	MPU6050_ReadRegByteData(MPU6050_WHO_AM_I, byte);		//返回WHO_AM_I寄存器的值
}



/**
  * 函    数：MPU6050获取数据
  * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
						int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
	uint8_t DataH, DataL;		//定义数据高8位和低8位的变量

	
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_XOUT_H,&DataH);		//读取加速度计X轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_XOUT_L,&DataL);		//读取加速度计X轴的低8位数据
	*AccX = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_YOUT_H,&DataH);		//读取加速度计Y轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_YOUT_L,&DataL);		//读取加速度计Y轴的低8位数据
	*AccY = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_ZOUT_H,&DataH);		//读取加速度计Z轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_ACCEL_ZOUT_L,&DataL);		//读取加速度计Z轴的低8位数据
	*AccZ = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	 MPU6050_ReadRegByteData(MPU6050_GYRO_XOUT_H,&DataH);		//读取陀螺仪X轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_GYRO_XOUT_L,&DataL);		//读取陀螺仪X轴的低8位数据
	*GyroX = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	 MPU6050_ReadRegByteData(MPU6050_GYRO_YOUT_H,&DataH);		//读取陀螺仪Y轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_GYRO_YOUT_L,&DataL);		//读取陀螺仪Y轴的低8位数据
	*GyroY = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	 MPU6050_ReadRegByteData(MPU6050_GYRO_ZOUT_H,&DataH);		//读取陀螺仪Z轴的高8位数据
	 MPU6050_ReadRegByteData(MPU6050_GYRO_ZOUT_L,&DataL);		//读取陀螺仪Z轴的低8位数据
	*GyroZ = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
}
